home *** CD-ROM | disk | FTP | other *** search
/ BBS in a Box 3 / BBS in a box - Trilogy III.iso / Files / Prog / D-G / GemsI / Src / MatrixInvert.c < prev    next >
Encoding:
C/C++ Source or Header  |  1992-06-16  |  4.8 KB  |  183 lines  |  [TEXT/MPS ]
  1. /*
  2. Matrix Inversion
  3. by Richard Carling
  4. from "Graphics Gems", Academic Press, 1990
  5. */
  6.  
  7. #define SMALL_NUMBER    1.e-8
  8. /* 
  9.  *   inverse( original_matrix, inverse_matrix )
  10.  * 
  11.  *    calculate the inverse of a 4x4 matrix
  12.  *
  13.  *     -1     
  14.  *     A  = ___1__ adjoint A
  15.  *         det A
  16.  */
  17.  
  18. #include "GraphicsGems.h"
  19. inverse( in, out ) Matrix4 *in, *out;
  20. {
  21.     int i, j;
  22.     double det, det4x4();
  23.  
  24.     /* calculate the adjoint matrix */
  25.  
  26.     adjoint( in, out );
  27.  
  28.     /*  calculate the 4x4 determinent
  29.      *  if the determinent is zero, 
  30.      *  then the inverse matrix is not unique.
  31.      */
  32.  
  33.     det = det4x4( in );
  34.  
  35.     if ( fabs( det ) < SMALL_NUMBER) {
  36.         printf("Non-singular matrix, no inverse!\n");
  37.         exit();
  38.     }
  39.  
  40.     /* scale the adjoint matrix to get the inverse */
  41.  
  42.     for (i=0; i<4; i++)
  43.         for(j=0; j<4; j++)
  44.         out->element[i][j] = out->element[i][j] / det;
  45. }
  46.  
  47.  
  48. /* 
  49.  *   adjoint( original_matrix, inverse_matrix )
  50.  * 
  51.  *     calculate the adjoint of a 4x4 matrix
  52.  *
  53.  *      Let  a   denote the minor determinant of matrix A obtained by
  54.  *           ij
  55.  *
  56.  *      deleting the ith row and jth column from A.
  57.  *
  58.  *                    i+j
  59.  *     Let  b   = (-1)    a
  60.  *          ij            ji
  61.  *
  62.  *    The matrix B = (b  ) is the adjoint of A
  63.  *                     ij
  64.  */
  65.  
  66. adjoint( in, out ) Matrix4 *in; Matrix4 *out;
  67. {
  68.     double a1, a2, a3, a4, b1, b2, b3, b4;
  69.     double c1, c2, c3, c4, d1, d2, d3, d4;
  70.     double det3x3();
  71.  
  72.     /* assign to individual variable names to aid  */
  73.     /* selecting correct values  */
  74.  
  75.     a1 = in->element[0][0]; b1 = in->element[0][1]; 
  76.     c1 = in->element[0][2]; d1 = in->element[0][3];
  77.  
  78.     a2 = in->element[1][0]; b2 = in->element[1][1]; 
  79.     c2 = in->element[1][2]; d2 = in->element[1][3];
  80.  
  81.     a3 = in->element[2][0]; b3 = in->element[2][1];
  82.     c3 = in->element[2][2]; d3 = in->element[2][3];
  83.  
  84.     a4 = in->element[3][0]; b4 = in->element[3][1]; 
  85.     c4 = in->element[3][2]; d4 = in->element[3][3];
  86.  
  87.  
  88.     /* row column labeling reversed since we transpose rows & columns */
  89.  
  90.     out->element[0][0]  =   det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
  91.     out->element[1][0]  = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
  92.     out->element[2][0]  =   det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
  93.     out->element[3][0]  = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
  94.         
  95.     out->element[0][1]  = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
  96.     out->element[1][1]  =   det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
  97.     out->element[2][1]  = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
  98.     out->element[3][1]  =   det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
  99.         
  100.     out->element[0][2]  =   det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
  101.     out->element[1][2]  = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
  102.     out->element[2][2]  =   det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
  103.     out->element[3][2]  = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
  104.         
  105.     out->element[0][3]  = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
  106.     out->element[1][3]  =   det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
  107.     out->element[2][3]  = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
  108.     out->element[3][3]  =   det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
  109. }
  110. /*
  111.  * double = det4x4( matrix )
  112.  * 
  113.  * calculate the determinent of a 4x4 matrix.
  114.  */
  115. double det4x4( m ) Matrix4 *m;
  116. {
  117.     double ans;
  118.     double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3,             d4;
  119.     double det3x3();
  120.  
  121.     /* assign to individual variable names to aid selecting */
  122.     /*  correct elements */
  123.  
  124.     a1 = m->element[0][0]; b1 = m->element[0][1]; 
  125.     c1 = m->element[0][2]; d1 = m->element[0][3];
  126.  
  127.     a2 = m->element[1][0]; b2 = m->element[1][1]; 
  128.     c2 = m->element[1][2]; d2 = m->element[1][3];
  129.  
  130.     a3 = m->element[2][0]; b3 = m->element[2][1]; 
  131.     c3 = m->element[2][2]; d3 = m->element[2][3];
  132.  
  133.     a4 = m->element[3][0]; b4 = m->element[3][1]; 
  134.     c4 = m->element[3][2]; d4 = m->element[3][3];
  135.  
  136.     ans = a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
  137.         - b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
  138.         + c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
  139.         - d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
  140.     return ans;
  141. }
  142.  
  143.  
  144.  
  145. /*
  146.  * double = det3x3(  a1, a2, a3, b1, b2, b3, c1, c2, c3 )
  147.  * 
  148.  * calculate the determinent of a 3x3 matrix
  149.  * in the form
  150.  *
  151.  *     | a1,  b1,  c1 |
  152.  *     | a2,  b2,  c2 |
  153.  *     | a3,  b3,  c3 |
  154.  */
  155.  
  156. double det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3 )
  157. double a1, a2, a3, b1, b2, b3, c1, c2, c3;
  158. {
  159.     double ans;
  160.     double det2x2();
  161.  
  162.     ans = a1 * det2x2( b2, b3, c2, c3 )
  163.         - b1 * det2x2( a2, a3, c2, c3 )
  164.         + c1 * det2x2( a2, a3, b2, b3 );
  165.     return ans;
  166. }
  167.  
  168. /*
  169.  * double = det2x2( double a, double b, double c, double d )
  170.  * 
  171.  * calculate the determinent of a 2x2 matrix.
  172.  */
  173.  
  174. double det2x2( a, b, c, d)
  175. double a, b, c, d; 
  176. {
  177.     double ans;
  178.     ans = a * d - b * c;
  179.     return ans;
  180. }
  181.  
  182.  
  183.